#!/usr/bin/env python3
from launch import LaunchDescription
from launch_ros.actions import Node
import yaml
import os 
import logging

logger = logging.getLogger("terminal_launch_logger")

def GetConfig():
    """读取并解析USV配置文件"""
    current_path = os.path.abspath('.')
    yaml_path = os.path.join(current_path, "USVNode/usv_status.yaml")
    
    if not os.path.exists(yaml_path):
        logger.error(f"配置文件未找到: {yaml_path}")
        raise FileNotFoundError(f"配置文件未找到: {yaml_path}")
    
    try:
        with open(yaml_path, 'r', encoding='utf-8') as file:
            yaml_content = ''.join(line for line in file if not line.strip().startswith('#'))
            return yaml.safe_load(yaml_content)
    except Exception as e:
        logger.error(f"配置文件解析失败: {str(e)}")
        raise
# example
# There have two node type : one is device node ;other is Monitor node
# Device node need to filed the all parameter exception tcp_serve_ip and tcp_server_port
# Monitor node only need to filed three parameter: node_type , tcp_server_ip and tcp_server_port
def generate_launch_description():
    launchDesc = LaunchDescription()
    config = GetConfig()
    total_ships = 0
    ship_types = config.get('ShipTypes', [])
    
    for ship_type in ship_types:
        ship_count = ship_type['count']
        positions = ship_type['positions']
        
        if len(positions) != ship_count:
            logger.warning(f"类型{ship_type['type']}配置的船数({ship_count})与位置数({len(positions)})不匹配")
            ship_count = min(ship_count, len(positions))  # 取最小值避免越界
        
        # 处理每艘船
        for idx in range(ship_count):
            pos_data = positions[idx]
            ship_num = idx  # 类别内编号：0,1,2,...
            global_ship_num = total_ships
            
            # 节点名称：类型_船号，如 usv0_1
            node_name = f"usv_{ship_type['type']}_{ship_num}"
            
            params = {
                'usv_type': ship_type['type'],  # 确保是整数
                'usv_num': ship_num,                 # 确保是整数
                'posx': float(ship_type['positions'][ship_num][0]),
                'posy': float(ship_type['positions'][ship_num][1]),
                'pre_phi': float(ship_type['positions'][ship_num][2]),
                'rand_num': ship_type['rand_num_start'] + ship_num,
                'node_frequency': ship_type['node_frequency']
            }

            # 创建ROS节点
            node = Node(
                package=ship_type['package'],
                name=node_name,
                #namespace = node_name,
                executable=ship_type['executable'],
                output='screen',
                parameters=[params]
            )
            
            launchDesc.add_action(node)
            total_ships += 1
            print(f"添加船: {node_name} | 类型:{ship_type['type']} | 序号:{ship_num} "
                        f"| 位置:({pos_data[0]}, {pos_data[1]}) | 航向:{pos_data[2]}°")
    
    logger.info(f"成功添加 {total_ships} 艘船")
    return launchDesc




